#include "RosInterface.h"
#include <srs_ui_pro/simulation.h>
#include <srs_msgs/GraspSubConfiguration.h>

RosInterface::RosInterface()
{
    pid_OR = -1;
    pid_RVIZ = -1;
    pid_IMAGE_VIEW = -1;

    detection_client = n.serviceClient<cob_object_detection_msgs::DetectObjects>("/object_detection/detect_object");
    getgraspconfigurations_client = n.serviceClient<srs_grasping::GetGraspConfigurations>("/get_grasp_configurations");
    getgraspsfromposition_client = n.serviceClient<srs_grasping::GetGraspsFromPosition>("/get_grasps_from_position");
    simulategrasp_client = n.serviceClient<srs_ui_pro::simulation>("/grasp_simulator");
    ik_client = n.serviceClient<srs_ui_pro::simulation>("/arm_kinematics/get_ik");
}

RosInterface::~RosInterface()
{
    //dtor
}

cob_object_detection_msgs::DetectionArray RosInterface::Detect()
{
     cob_object_detection_msgs::DetectObjects detection_msg;
     detection_msg.request.object_name.data = "milk";
     detection_client.call(detection_msg);
     return detection_msg.response.object_list;
}

std::vector<srs_msgs::GraspConfiguration> RosInterface::getGraspConfigurations(int object_id)
{
    srs_grasping::GetGraspConfigurations getgraspconfigurations_msg;
    getgraspconfigurations_msg.request.object_id = object_id;
    getgraspconfigurations_client.call(getgraspconfigurations_msg);
    return getgraspconfigurations_msg.response.grasp_configuration;
}

std::vector<srs_msgs::GraspSubConfiguration> RosInterface::getGraspsFromPosition(int object_id, geometry_msgs::Pose object_pose)
{
    srs_grasping::GetGraspsFromPosition getgraspsfromposition_msg;
    getgraspsfromposition_msg.request.object_id = object_id;
    getgraspsfromposition_msg.request.object_pose = object_pose;
    getgraspsfromposition_client.call(getgraspsfromposition_msg);
    return getgraspsfromposition_msg.response.grasp_configuration;
}

void RosInterface::move(std::string component, std::vector<float> values)
{
    if (component.compare("base") == 0)
        //TODO: RosInterface::move_base(value);
        printf("---\n");
    else
    {
        std::string topic_name = "/"+component+"_controller/command";
        ros::Publisher pub = n.advertise<trajectory_msgs::JointTrajectory>(topic_name, 1, true);

        trajectory_msgs::JointTrajectory traj;
        traj.header.stamp = ros::Time::now()+ros::Duration(0.01);
        traj.points.resize(1);
        traj.points[0].time_from_start = ros::Duration(3.0);

        if (component.compare("arm") == 0)
        {
            traj.joint_names.push_back("arm_1_joint");
            traj.joint_names.push_back("arm_2_joint");
            traj.joint_names.push_back("arm_3_joint");
            traj.joint_names.push_back("arm_4_joint");
            traj.joint_names.push_back("arm_5_joint");
            traj.joint_names.push_back("arm_6_joint");
            traj.joint_names.push_back("arm_7_joint");

            for(int i = 0; i < 7; i++)
            {
                traj.points[0].positions.push_back(values[i]);
                traj.points[0].velocities.push_back(0.0);
            }
        }
        else if (component.compare("sdh") == 0)
        {
            traj.joint_names.push_back("sdh_knuckle_joint");
            traj.joint_names.push_back("sdh_finger_12_joint");
            traj.joint_names.push_back("sdh_finger_13_joint");
            traj.joint_names.push_back("sdh_finger_22_joint");
            traj.joint_names.push_back("sdh_finger_23_joint");
            traj.joint_names.push_back("sdh_thumb_2_joint");
            traj.joint_names.push_back("sdh_thumb_3_joint");

            for(int i = 0; i < 7; i++)
            {
                traj.points[0].positions.push_back(values[i]);
                traj.points[0].velocities.push_back(0.0);
            }
        }
        else if (component.compare("torso") == 0)
        {
            traj.joint_names.push_back("arm_1_joint");
            traj.joint_names.push_back("arm_2_joint");
            traj.joint_names.push_back("arm_3_joint");
            traj.joint_names.push_back("arm_4_joint");
            traj.joint_names.push_back("arm_5_joint");
            traj.joint_names.push_back("arm_6_joint");
            traj.joint_names.push_back("arm_7_joint");

            for(int i = 0; i < 7; i++)
            {
                traj.points[0].positions.push_back(values[i]);
                traj.points[0].velocities.push_back(0.0);
            }
        }
        else if (component.compare("head") == 0)
        {
            traj.joint_names.push_back("head_axis_joint");
            traj.points[0].positions.push_back(values[0]);
            traj.points[0].velocities.push_back(0.0);
        }
        else //tray
        {
            traj.joint_names.push_back("torso_tray_joint");
            traj.points[0].positions.push_back(values[0]);
            traj.points[0].velocities.push_back(0.0);
        }
        pub.publish(traj);
        sleep(1);
    }//if-else
}

int RosInterface::run_OpenRave(int object_id)
{
	if((pid_OR = fork()) == -1)
	{
		printf("Error in fork: %s\n", strerror(errno));
		return PROCESS_ERROR_FORK; //Error al ejecutar fork
	}
	else if(pid_OR == 0)
	{ //child
		if(execlp("rosrun", "rosrun", "srs_grasping", "simulation.py", object_id, NULL) < 0)
		{
			printf("Error in exec: %s\n", strerror(errno));
			exit(1);
		}
	}
	else
		return PROCESS_OK;
}

int RosInterface::run_rviz()
{
	if((pid_RVIZ = fork()) == -1)
	{
		printf("Error in fork: %s\n", strerror(errno));
		return PROCESS_ERROR_FORK; //Error al ejecutar fork
	}
	else if(pid_RVIZ == 0)
	{ //child
		if(execlp("rosrun", "rosrun", "rviz", "rviz", NULL) < 0)
		{
			printf("Error in exec: %s\n", strerror(errno));
			exit(1);
		}
	}
	else
		return PROCESS_OK;
}

int RosInterface::image_view(char *topic)
{
    char arg[] = "image:=";
    strcat(arg, topic);

	if((pid_IMAGE_VIEW = fork()) == -1)
	{
		printf("Error in fork: %s\n", strerror(errno));
		return PROCESS_ERROR_FORK; //Error al ejecutar fork
	}
	else if(pid_IMAGE_VIEW == 0)
	{ //child
		if(execlp("rosrun", "rosrun", "image_view", "image_view", arg, NULL) < 0)
		{
			printf("Error in exec: %s\n", strerror(errno));
			exit(1);
		}
	}
	else
		return PROCESS_OK;
}

void RosInterface::joint_status()
{
    sub = n.subscribe("/joint_states", 1000, &RosInterface::callback, this);

    ros::Rate loop_rate(10);
    loop_rate.sleep();
    ros::spinOnce();

}

void RosInterface::callback(const sensor_msgs::JointState::ConstPtr &msg)
{
    if (msg->position.size() > 8)
    {
        joints = new joint_values[msg->position.size()];
        int arm_count, sdh_count, torso_count;
        arm_count=sdh_count=torso_count=0;

        for (unsigned int i=0; i<msg->position.size(); i++)
        {
            std::string joint_name = msg->name[i];
            if ((joint_name.substr(0,3)).compare("arm")==0)
            {                arm_joints[arm_count].name = msg->name[i];
                arm_joints[arm_count].position = msg->position[i];
                arm_joints[arm_count].velocity = msg->velocity[i];
                arm_count++;
            }
            else if (joint_name.compare("sdh_finger_21_joint")==0)
            {
                //None
            }
            else if ((joint_name.substr(0,3)).compare("sdh")==0)
            {
                sdh_joints[sdh_count].name = msg->name[i];
                sdh_joints[sdh_count].position = msg->position[i];
                sdh_joints[sdh_count].velocity = msg->velocity[i];
                sdh_count++;
            }
            else if (joint_name.compare("torso_tray_joint")==0)
            {
                tray_joints[0].name = msg->name[i];
                tray_joints[0].position = msg->position[i];
                tray_joints[0].velocity = msg->velocity[i];
            }
            else if ((joint_name.substr(0,5)).compare("torso")==0)
            {
                torso_joints[torso_count].name = msg->name[i];
                torso_joints[torso_count].position = msg->position[i];
                torso_joints[torso_count].velocity = msg->velocity[i];
                torso_count++;
            }
            else if ((joint_name.substr(0,4)).compare("head")==0)
            {
                head_joints[0].name = msg->name[i];
                head_joints[0].position = msg->position[i];
                head_joints[0].velocity = msg->velocity[i];
            }
            else
            {
            //joints[i].name = msg->name[i];
            //joints[i].position = msg->position[i];
            //joints[i].velocity = msg->velocity[i];
            //joints.effort.push_back(msg->effort[i]);
            }
        }
        sub.shutdown();
    }
}

RosInterface::joint_values * RosInterface::getJointValues(std::string component)
{
    if (component.compare("arm")==0)
        return arm_joints;
    else if (component.compare("sdh")==0)
        return sdh_joints;
    else if (component.compare("torso")==0)
        return torso_joints;
    else if (component.compare("tray")==0)
        return tray_joints;
    else if (component.compare("head")==0)
        return head_joints;
    else
        return joints;
}

std::string RosInterface::toString(RosInterface::joint_values jv)
{
    std::string res(std::string(jv.name)+"\t\t"+boost::lexical_cast<std::string>(jv.position)+"\t\t"+boost::lexical_cast<std::string>(jv.velocity));
    return res;
}

std::string RosInterface::toStringAll(RosInterface::joint_values *jv)
{
    std::string res, n, p, v;

    int size;
    std::string joint_name = jv[0].name;

    if (((joint_name).substr(0,3)).compare("arm")==0 || ((joint_name).substr(0,3)).compare("sdh")==0)
        size = 7;
    else if (joint_name.compare("torso_tray_joint")==0 || joint_name.compare("head_axis_joint")==0)
        size = 1;
    else
        size = 3;

    n = "[";
    for (int i=0; i<size; i++)
    {
        n += jv[i].name+",\t";

        std::string aux = (boost::lexical_cast<std::string>(jv[i].position)).substr(0,jv[i].name.size());
        p += aux+"\t\t";

        std::string aux2 = (boost::lexical_cast<std::string>(jv[i].velocity)).substr(0,jv[i].name.size());
        v += aux2+"\t\t";
    }
    n = n.substr(0,n.size()-2);
    n += "]";
    res = n+"\n"+p+"\n"+v;

    return res;
}

void RosInterface::simulate_grasp(int object_id, srs_msgs::GraspSubConfiguration grasp, geometry_msgs::Pose object_pose)
{
    srs_ui_pro::simulation graspsimulation_msg;
    graspsimulation_msg.request.object_id = object_id;
    graspsimulation_msg.request.object_pose = object_pose;
    graspsimulation_msg.request.grasp = grasp;
    simulategrasp_client.call(graspsimulation_msg);
}

geometry_msgs::Pose RosInterface::transformPose(geometry_msgs::PoseStamped p)
{
    geometry_msgs::PoseStamped result;
    tf::TransformListener listener(ros::Duration(10));
    listener.waitForTransform(p.header.frame_id, "/base_link", ros::Time::now(), ros::Duration(3.0));
    listener.transformPose("/base_link", p, result);
    return result.pose;
}

std::vector<float> RosInterface::callIkSolver(std::vector<float> current_joint_configuration, geometry_msgs::PoseStamped target_pose)
{

    kinematics_msgs::GetPositionIK ik_msg;

    ik_msg.request.ik_request.ik_link_name = "sdh_palm_link";
    ik_msg.request.ik_request.ik_seed_state.joint_state.position.resize(7);
    for (int i=0; i<current_joint_configuration.size(); i++)
        ik_msg.request.ik_request.ik_seed_state.joint_state.position[i] = current_joint_configuration[i];
    ik_msg.request.ik_request.pose_stamped = target_pose;

    ik_client.call(ik_msg);

    std::vector<float> res;
    res.resize(7);
    for (int i=0; i<ik_msg.response.solution.joint_state.position.size(); i++)
        res[i] = ik_msg.response.solution.joint_state.position[i];

    return res;
}


bool RosInterface::computeIK(std::vector<float> current_joint_configuration, const geometry_msgs::PoseStamped &pose, std::vector<float> &solution)
{
  kinematics_msgs::GetPositionIK::Request request;
  kinematics_msgs::GetPositionIK::Response response;

  request.ik_request.pose_stamped = pose;
  request.timeout = ros::Duration(2.0);

  request.ik_request.ik_seed_state.joint_state.position.resize(7);
  for(int j = 0 ; j < 7; ++j)
    request.ik_request.ik_seed_state.joint_state.position.push_back(current_joint_configuration[j]);


  if(ik_client.call(request, response))
  {
    if(response.error_code.val == response.error_code.SUCCESS)
      for(unsigned int i=0; i < response.solution.joint_state.name.size(); i++)
      {
        solution[i] = response.solution.joint_state.position[i];
      }
    else
    {
      ROS_ERROR("Inverse kinematics failed for %s. (error code: %d)", request.ik_request.ik_link_name.c_str(), response.error_code.val);
      return false;
    }
  }
  else
  {
    ROS_ERROR("IK service failed");
    return false;
  }
  return true;
}
